package simpleGeom;

import java.awt.geom.Line2D;
import java.util.List;

import processing.core.PMatrix;
import processing.core.PMatrix3D;
import processing.core.PVector;

public class DistanceToFaces {
	PMatrix[] matrices;
	Line3D[] lines1;
	Line3D[] lines2;
	Line3D[] lines3;

	public void calculateDistances(List<Face3D> faces, HasPos3D planeAxis) {
		matrices = new PMatrix[faces.size()];
		lines1 = new Line3D[faces.size()];
		lines2 = new Line3D[faces.size()];
		lines3 = new Line3D[faces.size()];
		for (int i = 0; i < faces.size(); i++) {
			Face3D f = faces.get(i);
			HasPos3D v1 = f.getNode(0);
			HasPos3D v2 = f.getNode(1);
			HasPos3D v3 = f.getNode(2);
			Vec3D v12 = Vec3D.sub(v2, v1);
			Vec3D v13 = Vec3D.sub(v3, v1);
			Vec3D n = Vec3D.cross(v12, v13);
			n.normalize();
			float angle = (float) Math.acos(n.dot(planeAxis));

			Vec3D u = n.cross(planeAxis);
			u.normalize();

			float c = (float) Math.cos(angle);
			float s = (float) Math.sin(angle);
			float t = 1f - c;
			PMatrix3D m = new PMatrix3D();

			m.m00 = c + u.getX() * u.getX() * t;
			m.m01 = u.getX() * u.getY() * t - u.getZ() * s;
			m.m02 = u.getX() * u.getZ() * t + u.getY() * s;

			m.m10 = u.getY() * u.getX() * t + u.getZ() * s;
			m.m11 = c + u.getY() * u.getY() * t;
			m.m12 = u.getY() * u.getZ() * t - u.getX() * s;

			m.m20 = u.getZ() * u.getX() * t - u.getY() * s;
			m.m21 = u.getZ() * u.getY() * t + u.getX() * s;
			m.m22 = c + u.getZ() * u.getZ() * t;

			matrices[i] = m;

			PVector[] pts = new PVector[3];
			float[] f1 = new float[] { v1.getX(), v1.getY(), v1.getZ() };
			float[] f2 = new float[] { v2.getX(), v2.getY(), v2.getZ() };
			float[] f3 = new float[] { v3.getX(), v3.getY(), v3.getZ() };

			float[] f1T = m.mult(f1, null);
			float[] f2T = m.mult(f2, null);
			float[] f3T = m.mult(f3, null);

			lines1[i] = new Line3D(f1T[0], f1T[1], f1T[2], f2T[0], f2T[1],
					f2T[2]);
			lines2[i] = new Line3D(f2T[0], f2T[1], f2T[2], f3T[0], f3T[1],
					f3T[2]);
			lines3[i] = new Line3D(f3T[0], f3T[1], f3T[2], f1T[0], f1T[1],
					f1T[2]);

		}
	}

	public float getDistance(float x, float y, float z) {
		float minD = Float.MAX_VALUE;
		float[] pt = new float[] { x, y, z };
		float[] trans = new float[3];
		for (int i = 0; i < matrices.length; i++) {
			PMatrix m = matrices[i];
			Line3D l1 = lines1[i];
			Line3D l2 = lines2[i];
			Line3D l3 = lines3[i];

			trans = m.mult(pt, null);
			float cX = trans[0];
			float cY = trans[1];
			float cZ = trans[2];
			/*
			 * float cX=x; float cY=y; float cZ=z;
			 */

			int r1 = Line2D.relativeCCW(l1.x1, l1.y1, l1.x2, l1.y2, cX, cY);
			int r2 = Line2D.relativeCCW(l2.x1, l2.y1, l2.x2, l2.y2, cX, cY);
			int r3 = Line2D.relativeCCW(l3.x1, l3.y1, l3.x2, l3.y2, cX, cY);
			float cDistance;
			if (r1 == r2 && r1 == r3) {
				cDistance = (cZ - l1.z1) * (cZ - l1.z1);
				if (cDistance < minD) {
					minD = cDistance;
				}
			} else {
				float d1 = (float) l1.ptSegDistSq(cX, cY, cZ);
				float d2 = (float) l2.ptSegDistSq(cX, cY, cZ);
				float d3 = (float) l3.ptSegDistSq(cX, cY, cZ);
				if (d1 < minD) {
					minD = d1;
				}
				if (d2 < minD) {
					minD = d2;
				}
				if (d3 < minD) {
					minD = d3;
				}
			}
		}
		return (float) Math.sqrt(minD);
	}

	public PMatrix3D getRotateMatrix(float angle, HasPos3D u) {
		float c = (float) Math.cos(angle);
		float s = (float) Math.sin(angle);
		float t = 1f - c;
		PMatrix3D m = new PMatrix3D();

		m.m00 = c + u.getX() * u.getX() * t;
		m.m01 = u.getX() * u.getY() * t - u.getZ() * s;
		m.m02 = u.getX() * u.getZ() * t + u.getY() * s;

		m.m10 = u.getY() * u.getX() * t + u.getZ() * s;
		m.m11 = c + u.getY() * u.getY() * t;
		m.m12 = u.getY() * u.getZ() * t - u.getX() * s;

		m.m20 = u.getZ() * u.getX() * t - u.getY() * s;
		m.m21 = u.getZ() * u.getY() * t + u.getX() * s;
		m.m22 = c + u.getZ() * u.getZ() * t;

		return m;
	}

	public PMatrix3D getRotateMatrix(float angle, PVector u) {
		float c = (float) Math.cos(angle);
		float s = (float) Math.sin(angle);
		float t = 1f - c;
		PMatrix3D m = new PMatrix3D();

		m.m00 = c + u.x * u.x * t;
		m.m01 = u.x * u.y * t - u.z * s;
		m.m02 = u.x * u.z * t + u.y * s;

		m.m10 = u.y * u.x * t + u.z * s;
		m.m11 = c + u.y * u.y * t;
		m.m12 = u.y * u.z * t - u.x * s;

		m.m20 = u.z * u.x * t - u.y * s;
		m.m21 = u.z * u.y * t + u.x * s;
		m.m22 = c + u.z * u.z * t;

		return m;
	}

	public float distPtToSegment(HasPos3D p, HasPos3D s1, HasPos3D s2) {
		Vec3D v = Vec3D.sub(s1, s2);
		Vec3D w = Vec3D.sub(p, s1);

		float c1 = Vec3D.dot3D(w, v);
		if (c1 <= 0)
			return Vec3D.distSqr(s1, p);

		float c2 = Vec3D.dot3D(v, v);
		if (c2 <= c1)
			return Vec3D.distSqr(p, s2);

		float b = c1 / c2;
		v.mult(b);
		Vec3D Pb = Vec3D.add(s1, v);
		return Vec3D.distSqr(p, Pb);
	}

	public PVector[] rotateTriangle(PVector v1, PVector v2, PVector v3,
			PVector planeAxis) {
		PVector v12 = PVector.sub(v2, v1);
		PVector v13 = PVector.sub(v3, v1);
		PVector n = v12.cross(v13);
		n.normalize();

		float angle = (float) Math.acos(n.dot(planeAxis));
		PVector rotAxis = n.cross(planeAxis);
		rotAxis.normalize();
		PMatrix3D m = getRotateMatrix(angle, rotAxis);

		PVector[] pts = new PVector[3];
		pts[0] = m.mult(v1, pts[0]);
		pts[1] = m.mult(v2, pts[1]);
		pts[2] = m.mult(v3, pts[2]);

		return pts;
	}
}
